#include "CProtocol.h"

//globals
CProtocol g_myPrt;
CProtocol g_otherPrt;
int g_imCurrentlyWriting;
int g_imCurrentlyListening;

void commitProtocol(CProtocol* p) {
	byte bits = 0;
	bits |= p->readStatus << 3;
	bits |= p->writeStatus << 4;
	bits |= p->data & 1;
	bits |= p->data & 2;
	if(p->useCustomData) {
		bits |= (p->customData & 1) << 2;
	} else {
		bits |= p->data & 4;
	}
	outb(bits, 0x378);
}

void readProtocol(CProtocol* out) {
	byte bits;
	bits = inb(0x379);
	// shift-left bits since bits are read shift-righted
	bits >>= 3;
	bits ^= (1 << 4);
	out->readStatus		= (bits & (1 << 3)) >> 3;
	out->writeStatus	= (bits & (1 << 4)) >> 4;
	out->data 		= get1stThreeBit(bits);
}

void readProtocolWithCustomData(CProtocol* out) {
	byte bits;
	bits = inb(0x379);
	// shift-left bits since bits are read shift-righted
	// shift-left bits since bits are read shift-righted
	bits >>= 3;
	bits ^= (1 << 4);
	out->readStatus		= (bits & (1 << 3)) >> 3;
	out->writeStatus	= (bits & (1 << 4)) >> 4;
	byte data_ = 0;
	data_ |= bits & 3;
	out->data = data_;
	out->customData = (bits & (1 << 2)) >> 2;
}

byte get1stThreeBit(byte from) {
	return from & 7;
}

byte get2ndThreeBit(byte from) {
	return (from >> 3) & 7;
}

byte getLastTwoBit(byte from) {
	return (from >> 6) & 7;
}

// set bit 1, 2 and 3 of from to val
void set1stThreeBit(byte* from, byte val) {
	// set first 3 bit to 0
	*from = *from & -8;
	// set first 3 bit to val
	*from = *from | (val & 7);
}

// set bit 4, 5 and 6 of from to val
void set2ndThreeBit(byte* from, byte val) {
	// set second 3 bit to 0
	*from = *from & -57;
	// set first 3 bit to val
	*from = *from | ((val & 7) << 3);
}

// set bit 7 and 8 of from to val
void setLastTwoBit(byte* from, byte val) {
	// set bit 7 and 8 to 0
	*from = *from & -193;
	// set bit 7 and 8 to val
	*from = *from | ((val & 7) << 6);
}

//Atomic Test-and-set-lock
int TSL(int* lock) {
    return 1;
}

void releaseLock(int* lock) {

}

unsigned long getMillisecs(void) {
	unsigned long result;
	rdtscl(result);
	return result;
}

int tooLong(unsigned long startTime) {
	return getMillisecs() - startTime > 4000000000LL;
}

void initCProtocol() {
	g_myPrt.writeStatus 	= 0;
	g_myPrt.readStatus 	= 0;
	g_myPrt.useCustomData 	= 0;
	commitProtocol(&g_myPrt);
}

int readAChar(byte* out, byte* outCustomData) {
	//notify other I'm listening
	g_myPrt.readStatus = 1;
	commitProtocol(&g_myPrt);
	
	//read each bits, we expect the first writeStatus is 1
	int prevWriteStatus = 0;
	int i = 0;
	unsigned long time = getMillisecs();
	for(; i < 3; ++i) {
		//Wait until bits is sent by other
		int isTimeout = 0;
		do {
			isTimeout = tooLong(time);
			readProtocol(&g_otherPrt);
		} while(!isTimeout && g_otherPrt.writeStatus == prevWriteStatus);
		prevWriteStatus = g_otherPrt.writeStatus;
		
		//quit if timeout
		if(isTimeout) break;
		
		//read from other based on current iteration
		if(i == 0) {
			set1stThreeBit(out, g_otherPrt.data);
		} else if(i == 1) {
			set2ndThreeBit(out, g_otherPrt.data);
		} else {
			readProtocolWithCustomData(&g_otherPrt);
			*outCustomData = g_otherPrt.customData;
			setLastTwoBit(out, g_otherPrt.data);
		}
		
		//send ack by toggling read status
		g_myPrt.readStatus = !g_myPrt.readStatus;
		printk("<1> >> %d \n", g_otherPrt.data);
		commitProtocol(&g_myPrt);
	}
	
	//reset variables
	g_myPrt.readStatus = 0;
	commitProtocol(&g_myPrt);
	
	//done!
	return i;
}

int writeAChar(byte in, byte customData) {
	//if other is currently not listening, wait
	unsigned long time = getMillisecs();
	do {
		if(tooLong(time)) return 0; //fail
		readProtocol(&g_otherPrt);
	} while(g_otherPrt.readStatus != 1);

	//send bits to other
	int prevReadStatus = g_otherPrt.readStatus;
	g_myPrt.writeStatus= 0;
	int i = 0;
	for(; i < 3; ++i) {
		//set bits to send based on cur iteration
		if(i == 0) {
			g_myPrt.data = get1stThreeBit(in); 
			g_myPrt.useCustomData = 0;
		} else if(i == 1) {
			g_myPrt.data = get2ndThreeBit(in); 
			g_myPrt.useCustomData = 0;
		} else {
			g_myPrt.data = getLastTwoBit(in); 
			g_myPrt.useCustomData = 1;
			g_myPrt.customData = customData;
		}
		
	 	//commit a new write by toggling writeStatus
		commitProtocol(&g_myPrt);
		g_myPrt.writeStatus = !g_myPrt.writeStatus;
		commitProtocol(&g_myPrt);
		
		//wait for ack
		int isTimeout = 0;
		time = getMillisecs();
		do {
			isTimeout = tooLong(time);
			readProtocol(&g_otherPrt);
		} while(!isTimeout && g_otherPrt.readStatus == prevReadStatus);
		prevReadStatus = g_otherPrt.readStatus;
		
		//break if timeout
		if(isTimeout) break;
	}
	
	//reset variables
	g_myPrt.writeStatus = 0;
	commitProtocol(&g_myPrt);
	
	//done!
	return i;
}

ssize_t readABunch(byte* out, byte* foundEOT, size_t len) {
	printk("<1> Beginning readABunch\n");
	//test and set lock
	if(!TSL(&g_imCurrentlyListening))
		return -1;
		
	//read data
	ssize_t i = 0;
	byte EOT = 0;
	byte got = 0;
	byte checkSum = 0;
	int succ = 1;
	printk("<1> Beginning for readABunch\n");
	for( ; succ && (!EOT) && (i < len); ++i) {
		succ = readAChar(&got, &EOT) == 3;
		printk("<1> EOT = %d; got = %c; succ = %d\n", EOT, got, succ);
		checkSum = (checkSum + got) % 100;
		out[i] = got;
	}
	
	//reading a bunch done, release lock
	releaseLock(&g_imCurrentlyListening);
	printk("<1> Finished readABunch, succ = %d\n", succ ? i : 0);
	*foundEOT = EOT;
	return succ ? i : 0;
}

ssize_t writeABunch(byte* in, size_t len) {
	printk("<1> Beginning writeABunch\n");
	//test and set lock
	if(!TSL(&g_imCurrentlyWriting))
		return -1;
		
	//write data
	ssize_t i = 0;
	byte checkSum = 0;
	int succ = 1;
	printk("<1> Beginning for writeABunch\n");
	for( ; succ && i < len; ++i) {
		succ = writeAChar(in[i], i == len-1 ? 1 : 0) == 3; //send 1 as custom data if this is the last string
		checkSum += in[i];
	}
	
	//reading a bunch succeed, release lock
	releaseLock(&g_imCurrentlyWriting);
	printk("<1> Finished writeABunch, succ = %d\n", succ ? i : 0);
	return succ ? i : 0;
	//return 1;
}
